#include <assert.h>

#include "PolygonGenerator.h"
#include "geometricUtility.h"
#include "OsgUtil.h"

int findIndex(const osg::Vec3d& point, std::vector<osg::Vec3d>& points)
{
	for (int i = 0; i < points.size(); ++i)
	{
		if (pointApprox(point, points[i]))
			return i;
	}
	points.push_back(point);
	return points.size() - 1;
}

int findPlaneIndex(const Plane& p1, std::vector<Plane>& planes)
{
	for (int i = 0; i < planes.size(); ++i)
	{
		if (isSamePlane(p1, planes[i]))
			return i;
	}
	planes.push_back(p1);
	return planes.size() - 1;
}

void removeDanglingConnections(std::map<int, std::vector<int>>& connections)
{
	std::map<int, std::vector<int>> tmp;
	for (const auto& pair : connections)
	{
		if (pair.second.size() >= 2)
		{
			tmp.insert(pair);
		}
	}
	if (connections.size() == tmp.size())
		return;
	connections = tmp;
	removeDanglingConnections(connections);
}

std::map<int, std::vector<int>> removeOnePointFromConnections(int ptIndex, const std::map<int, std::vector<int>>& connections)
{
	std::map<int, std::vector<int>> newConnections(connections);
	newConnections.erase(ptIndex);
	for (auto& pair : newConnections)
	{
		auto it = std::find(pair.second.begin(), pair.second.end(), ptIndex);
		if (it != pair.second.end())
			pair.second.erase(it);
	}
	return newConnections;
}

std::vector<std::vector<int>> findAllPathsBetween(int ptIndex1, int ptIndex2, const std::map<int, std::vector<int>>& planeConnections)
{
	std::vector<std::vector<int>> result;
	std::stack<int> mainStack;
	std::stack<std::vector<int>> assistStack;
	std::set<int> visit;
	mainStack.push(ptIndex1);
	visit.insert(ptIndex1);
	auto it = planeConnections.find(ptIndex1);
	if (it == planeConnections.end())
		return result;
	assistStack.push(it->second);
	while (mainStack.size() != 0)
	{
		//获取当前节点的下一节点
		auto nextPointIndice = assistStack.top();
		//去掉已经走过的节点
		for (auto it = nextPointIndice.begin(); it != nextPointIndice.end();)
		{
			auto it1 = visit.find(*it);
			if (it1 != visit.end())
			{
				//说明已经走过该节点
				it = nextPointIndice.erase(it);
			}
			else
			{
				it++;
			}
		}
		//还可以继续往下走
		if (nextPointIndice.size() > 0)
		{
			int index = nextPointIndice.back();
			nextPointIndice.pop_back();
			assistStack.pop();
			assistStack.push(nextPointIndice);
			//继续向栈内压入节点
			mainStack.push(index);
			visit.insert(index);
			auto it = planeConnections.find(index);
			//当前主栈节点无下一连接
			if (it == planeConnections.end())
			{
				mainStack.pop();
				visit.erase(index);
			}
			else
			{
				assistStack.push(it->second);
			}
			
		}
		else
		{
			int topIndex = mainStack.top();
			mainStack.pop();
			visit.erase(topIndex);
			assistStack.pop();
		}
		//找到目标路径
		if (mainStack.size() > 0 && mainStack.top() == ptIndex2)
		{
			std::vector<int> vec;
			std::stack<int> temp(mainStack);
			while (!temp.empty())
			{
				vec.push_back(temp.top());
				temp.pop();
			}
			std::reverse(vec.begin(), vec.end());
			result.push_back(vec);
			int topIndex = mainStack.top();
			mainStack.pop();
			visit.erase(topIndex);
			assistStack.pop();
		}
	}
	return result;
}

std::vector<std::vector<int>> findAllLoopsStartWith(int thisPointIndex, const std::map<int, std::vector<int>>& planeConnections)
{
	std::vector<std::vector<int>> result;
	const std::vector<int>& thisPointConnections = planeConnections.at(thisPointIndex);
	for (int j = 0; j < int(thisPointConnections.size() - 1); ++j)
	{
		for (int k = j + 1; k < thisPointConnections.size(); ++k)
		{
			auto newConnections = removeOnePointFromConnections(thisPointIndex, planeConnections);
			// Dont call removeDanglingConnections(newConnections);
			auto paths = findAllPathsBetween(thisPointConnections[j], thisPointConnections[k], newConnections);
			for (auto& path : paths)
			{
				path.push_back(thisPointIndex);
				result.push_back(path);
			}
		}
	}
	return result;
}


std::vector<std::vector<int>> findAllLoops(const std::map<int, std::vector<int>>& planeConnections)
{
	std::vector<std::vector<int>> result;
	if (planeConnections.size() < 3)
		return result;
	const auto& pair = *planeConnections.begin();
	result = findAllLoopsStartWith(pair.first, planeConnections);

	// remove the first point from connections, then recursively call findAllLoops
	auto newPlaneConnections = removeOnePointFromConnections(pair.first, planeConnections);
	removeDanglingConnections(newPlaneConnections);
	std::vector<std::vector<int>> result2 = findAllLoops(newPlaneConnections);
	result.insert(result.end(), result2.begin(), result2.end());
	return result;
}

//求某顶点相对于所判点所处象限
int quad(const osg::Vec2d& po, const osg::Vec2d& pt)
{
	int re;
	if (pt.x() < po.x())
	{
		if (pt.y() < po.y())
			re = 2;
		else
			re = 1;
	}
	else
	{
		if (pt.y() < po.y())
			re = 3;
		else
			re = 0;
	}
	return re;
}

// //polygon可以是凹多边形
bool PtInPolygonEx(const std::vector<osg::Vec2d>& polygon, const osg::Vec2d& pt)
{
	int n = polygon.size();
	int oldquad, newquad;
	int i, wind = 0;;
	int lastpt = n - 1;
	oldquad = quad(pt, polygon[lastpt]);
	for (i = 0; i < n; i++)
	{
		newquad = quad(pt, polygon[i]);
		if (newquad != oldquad)
		{
			if (((oldquad + 1) & 3) == newquad)
				wind++;
			else if (((newquad + 1) & 3) == oldquad)
				wind--;
			else
			{
				double a[2], b[2];
				a[0] = polygon[lastpt].x() - pt.x();
				a[1] = polygon[lastpt].y() - pt.y();
				b[0] = polygon[i].x() - pt.x();
				b[1] = polygon[i].y() - pt.y();
				double c = CrossProduct(a, b);
				if (c > 0)
					wind += 2;
				else
					wind -= 2;
			}
		}
		lastpt = i;
		oldquad = newquad;
	}

	return wind != 0;
}

bool isInnerEdge(const std::vector<int>& loop, int from, int to)
{
	auto it = std::find(loop.begin(), loop.end(), from);
	if (it == loop.end())
		return false;
	auto it2 = std::find(loop.begin(), loop.end(), to);
	if (it2 == loop.end())
		return false;
	int i = it - loop.begin();
	int j = it2 - loop.begin();
	if (std::abs(i - j) == 1)
		return false;
	if (std::abs(i - j) == int(loop.size() - 1))
		return false;
	return true;
}

bool isMesh(const std::vector<int>& loop, const std::set<int>& planePointIndice, const std::vector<osg::Vec3d>& points, const std::map<int, std::vector<int>>& planeConnections, const osg::Matrixd& planeMatrix_1, std::vector<osg::Vec2d>& point2Ds)
{
	// 判断有没有内部边
	for (const auto& pair : planeConnections)
	{
		int ptIndex1 = pair.first;
		for (int ptIndex2 : pair.second)
		{
			if (ptIndex1 < ptIndex2) // 消除重复的边判断
				if (isInnerEdge(loop, ptIndex1, ptIndex2))
					return false;
		}
	}

	// 判断有没有内部点
	std::vector<osg::Vec3d> polygon(loop.size());
	for (int i = 0; i < loop.size(); ++i)
		polygon[i] = points[loop[i]];
	std::set<int> otherPointIndice(planePointIndice);
	for (int index : loop)
	{
		auto it = otherPointIndice.find(index);
		if (it != otherPointIndice.end())
			otherPointIndice.erase(index);
	}
	for (int i = 0; i < polygon.size(); ++i)
	{
		const auto& pt3D = polygon[i];
		auto pt2D = pt3D * planeMatrix_1;
		point2Ds.push_back(osg::Vec2d(pt2D.x(), pt2D.y()));
	}
	for (int index : otherPointIndice)
	{
		// 不支持带孔的结构
		auto pt2D = points[index] * planeMatrix_1;
		bool inPolygon = PtInPolygonEx(point2Ds, osg::Vec2d(pt2D.x(), pt2D.y()));
		if (inPolygon)
		{
			//判断该点是否与环上点有连接
			const auto& pair = planeConnections.find(index);
			bool flag = false;
			if (pair != planeConnections.end())
			{
				auto thisPointConnnections = pair->second;
				for (const auto& index1 : loop)
				{
					auto it = std::find(thisPointConnnections.begin(), thisPointConnnections.end(), index1);
					if (it != thisPointConnnections.end())
					{
						flag = true;
						break;
					}			
				}
			}
			if (flag)
				return false;
		}
			
	}
	return true;
}

std::vector<std::shared_ptr<LineStrip3D>> removeZeroLines(const std::vector<std::shared_ptr<LineStrip3D>>& lines)
{
	std::vector<std::shared_ptr<LineStrip3D>> result;
	for (const auto& line : lines)
	{
		const auto& pts = line->getPoints();
		if (pts.size() < 2)
			continue;
		std::vector<int> removedPtIndice;
		for (int i = 0; i < int(pts.size() - 1); ++i)
		{
			if (pointApprox(pts[i], pts[i + 1]))
			{
				removedPtIndice.push_back(i + 1);
			}
		}
		if (removedPtIndice.empty())
		{
			result.push_back(line);
		}
		else
		{
			if (pts.size() - removedPtIndice.size() >= 2)
			{
				std::vector<osg::Vec3d> pts2;
				int start = 0;
				for (int i = 0; i < removedPtIndice.size(); ++i)
				{
					for (int j = start; j < removedPtIndice[i]; ++j)
					{
						pts2.push_back(pts[j]);
					}
					start = removedPtIndice[i] + 1;
				}
				for (int j = start; j < pts.size(); ++j)
				{
					pts2.push_back(pts[j]);
				}
				auto line2 = std::static_pointer_cast<LineStrip3D>(line->clone());
				result.push_back(line2);
			}
		}
	}
	return result;
}

std::vector<std::shared_ptr<LineStrip3D>> explodeLines(const std::vector<std::shared_ptr<LineStrip3D>>& lines)
{
	std::vector<std::shared_ptr<LineStrip3D>> result;
	for (const auto& line : lines)
	{
		std::vector<std::shared_ptr<Ent>> ents;
		bool explode = line->explode(ents);
		if (explode)
		{
			for (const auto ent : ents)
			{
				auto line = std::static_pointer_cast<LineStrip3D>(ent);
				result.push_back(line);
			}
		}
		else
		{
			result.push_back(line);
		}
	}
	return result;
}

bool isSameLine(const std::shared_ptr<LineStrip3D>& l1, const std::shared_ptr<LineStrip3D>& l2)
{
	assert(l1->getPoints().size() == 2 && l2->getPoints().size() == 2);
	if (l1->getPoints().size() == 2 && l2->getPoints().size() == 2)
	{
		const auto points1 = l1->getPoints();
		const auto points2 = l2->getPoints();
		if (pointApprox(points1[0], points2[0]) && pointApprox(points1[1], points2[1]))
			return true;
		if (pointApprox(points1[0], points2[1]) && pointApprox(points1[1], points2[0]))
			return true;
	}
	return false;
}

std::vector<std::shared_ptr<LineStrip3D>> removeDuplicatedLines(const std::vector<std::shared_ptr<LineStrip3D>>& lines)
{
	std::vector<std::shared_ptr<LineStrip3D>> result;
	for (const auto& line : lines)
	{
		bool isDifferent = true;
		for (const auto& line2 : result)
		{
			if (isSameLine(line, line2))
			{
				isDifferent = false;
				break;
			}
		}
		if (isDifferent)
			result.push_back(line);
	}
	return result;
}

struct PolygonData
{
	std::vector<int> pointIndice;
	std::vector<osg::Vec2d> point2Ds;
	int planeIndex = -1;
	bool processed = false;
};

void addEdgeDataOfPolygon(PolygonData& pd, std::map<std::pair<int, int>, bool>& edgeData)
{
	pd.processed = true;
	for (int i = 0; i < pd.pointIndice.size(); ++i)
	{
		int from = pd.pointIndice[i];
		int to = pd.pointIndice[(i + 1) % pd.pointIndice.size()];
		bool reversed = from > to;
		if (reversed)
			std::swap(from, to);
		auto edge = std::make_pair(from, to);
		edgeData.insert(std::make_pair(edge, reversed));
	}
}

bool adjustPolygonWinding(PolygonData& pd, std::map<std::pair<int, int>, bool>& edgeData)
{
	for (int i = 0; i < pd.pointIndice.size(); ++i)
	{
		int from = pd.pointIndice[i];
		int to = pd.pointIndice[(i + 1) % pd.pointIndice.size()];
		bool reversed = from > to;
		if (reversed)
			std::swap(from, to);
		auto edge = std::make_pair(from, to);
		auto it = edgeData.find(edge);
		if (it != edgeData.end())
		{
			// 边的绕向相同，需要取反
			if (reversed == it->second)
			{
				std::reverse(pd.pointIndice.begin(), pd.pointIndice.end());
				std::reverse(pd.point2Ds.begin(), pd.point2Ds.end());
			}
			addEdgeDataOfPolygon(pd, edgeData);
			return true;
		}
	}
	return false;
}

void makePolygonsWindingCoincident(std::vector<PolygonData>& polygonDatas)
{
	// pair.first < pair.second, bool is true means there is an edge from first to second. false mean reverse.
	std::map<std::pair<int, int>, bool> edgeData;
	int unprocessedPolygonCount = polygonDatas.size();
	bool newIndependentGroup = true;
	while (unprocessedPolygonCount > 0)
	{
		int firstUnprocessedPolygonIndex = -1;
		for (int i = 0; i < polygonDatas.size(); ++i)
		{
			if (!polygonDatas[i].processed)
			{
				firstUnprocessedPolygonIndex = i;
				break;
			}
		}
		if (firstUnprocessedPolygonIndex != -1)
		{
			if (newIndependentGroup)
				addEdgeDataOfPolygon(polygonDatas[firstUnprocessedPolygonIndex], edgeData);
		}
		int processedCount = 0;
		for (int i = firstUnprocessedPolygonIndex + 1; i < polygonDatas.size(); ++i)
		{
			if (!polygonDatas[i].processed)
			{
				bool processed = adjustPolygonWinding(polygonDatas[i], edgeData);
				if (processed)
					++processedCount;
			}
		}

		newIndependentGroup = processedCount == 0;

		// 计数没处理的多边形
		unprocessedPolygonCount = 0;
		for (const auto& pd : polygonDatas)
		{
			if (!pd.processed)
				++unprocessedPolygonCount;
		}
	} 
}

std::vector<std::shared_ptr<Polygon>> generatePolygons(const std::vector<std::shared_ptr<LineStrip3D>>& lines, Scene* scene)
{
	auto lines0 = removeZeroLines(lines);
	auto lines1 = explodeLines(lines0);
	auto lines2 = removeDuplicatedLines(lines1);
	// 1. construct graph
	std::vector<osg::Vec3d> points;
	std::map<int, std::vector<int>> connnections;
	// collect vetexes and connections
	for (const auto& line : lines2)
	{
		const auto& pts = line->getPoints();
		if (pts.size() < 2)
			continue;
		// fill points and indice
		std::vector<int> indice;
		for (int i = 0; i < pts.size(); ++i)
		{
			int index1 = findIndex(pts[i], points);
			indice.push_back(index1);
		}
		// fill connnections
		for (int i = 0; i < pts.size(); ++i)
		{
			int index1 = indice[i];
			if (i > 0)
			{
				connnections[index1].push_back(indice[i - 1]);
			}
			if (i < int(pts.size() - 1))
			{
				connnections[index1].push_back(indice[i + 1]);
			}
		}
	}

	removeDanglingConnections(connnections);

	// 2. find all different planes and their point indice
	std::vector<Plane> planes;
	std::vector<std::set<int>> allPlanePointIndice;
	for (int i = 0; i < int(points.size() - 2); ++i)
	{
		for (int j = 0; j < int(connnections[i].size() - 1); ++j)
		{
			for (int k = j + 1; k < connnections[i].size(); ++k)
			{
				Plane plane;
				bool success = getPlane(points[i], points[connnections[i][j]], points[connnections[i][k]], plane);
				if (success)
				{
					int index1 = findPlaneIndex(plane, planes);
					allPlanePointIndice.resize(planes.size());
					allPlanePointIndice[index1].insert(i);
					allPlanePointIndice[index1].insert(connnections[i][j]);
					allPlanePointIndice[index1].insert(connnections[i][k]);
				}
			}
		}
	}
	std::vector<std::map<int, std::vector<int>>> allPlanesConnections;
	allPlanesConnections.resize(planes.size());
	for (int i = 0; i < allPlanePointIndice.size(); ++i)
	{
		auto& thisPlaneConnections = allPlanesConnections[i];
		const auto& pointIndiceOfThisPlane = allPlanePointIndice[i];
		for (int ptIndex : pointIndiceOfThisPlane)
		{
			const auto& thisPointConnections = connnections[ptIndex];
			for (int otherEndIndex : thisPointConnections)
			{
				auto it = pointIndiceOfThisPlane.find(otherEndIndex);
				if (it != pointIndiceOfThisPlane.end())
					thisPlaneConnections[ptIndex].push_back(otherEndIndex);
			}
		}
	}

	std::vector<PolygonData> polygonDatas;
	for (int i = 0; i < allPlanesConnections.size(); ++i)
	{
		auto& thisPlaneConnections = allPlanesConnections[i];
		removeDanglingConnections(thisPlaneConnections);
		const std::set<int>& planePointIndice = allPlanePointIndice[i];
		auto thisPlaneLoops = findAllLoops(thisPlaneConnections);
		osg::Matrixd planeMatrix_1 = osg::Matrixd::inverse(planes[i].getMatrix());
		for (auto& loop : thisPlaneLoops)
		{
			std::vector<osg::Vec2d> point2Ds;
			bool result = isMesh(loop, planePointIndice, points, thisPlaneConnections, planeMatrix_1, point2Ds);
			if (result)
			{
				PolygonData polygonData;
				polygonData.planeIndex = i;
				polygonData.pointIndice = loop;
				polygonData.point2Ds.swap(point2Ds);
				polygonDatas.push_back(polygonData);
			}
		}
	}

	// 使所有多边形绕向一致。原则是同一边在它相关的两个多边形方向相反。
	makePolygonsWindingCoincident(polygonDatas);

	std::vector<std::shared_ptr<Polygon>> polygons;
	for (const auto& polygonData : polygonDatas)
	{
		auto polygon = std::make_shared<Polygon>(scene);
		polygon->setPoints(polygonData.point2Ds);
		const auto& plane = planes[polygonData.planeIndex];
		polygon->setOrigin(plane.origin());
		polygon->setAxis(plane.xAxis(), plane.yAxis());
		polygons.push_back(polygon);
	}

	return polygons;
}

